#pragma once
/**
 ********************************************************************************************************
 *                                               示例代码
 *                                             EXAMPLE  CODE
 *
 *                      (c) Copyright 2024; SaiShu.Lcc.; Leo; https://bjsstech.com
 *                                   版权所属[SASU-北京赛曙科技有限公司]
 *
 *            The code is for internal use only, not for commercial transactions(开源学习,请勿商用).
 *            The code ADAPTS the corresponding hardware circuit board(代码适配百度Edgeboard-智能汽车赛事版),
 *            The specific details consult the professional(欢迎联系我们,代码持续更正，敬请关注相关开源渠道).
 *********************************************************************************************************
 * @file crossroad.cpp
 * @author Leo
 * @brief 十字道路识别与图像处理
 * @version 0.1
 * @date 2022-03-14
 *
 * @copyright Copyright (c) 2022
 *
 * @note 十字道路处理步骤：
 *                      [01] 入十字类型识别：tracking.cpp
 *                      [02] 补线起止点搜索
 *                      [03] 边缘重计算
 *        spurroad岔路的x相当于图的y,岔路的y相当于点的x (都tm是反的)
 *        图像的左上角的x,y为(0,0),x为横,y为竖
 *        点集合顺序是按x从大到小排列的
 */

#include <fstream>
#include <iostream>
#include <cmath>
#include <opencv2/highgui.hpp>
#include <opencv2/opencv.hpp>
#include "../../include/common.hpp"
#include "tracking.cpp"

using namespace cv;
using namespace std;

class Crossroad
{
public:
    /**
     * @brief 初始化
     *
     */
    void reset(void)
    {
        crossroadType = CrossroadType::None; // 十字道路类型
    }

    /**
     * @brief 十字道路识别与图像处理
     *
     * @param track 赛道识别结果
     * @param imagePath 输入图像
     */
    bool crossRecognition(Tracking &track)
    {
        bool repaired = false;               // 十字识别与补线结果
        crossroadType = CrossroadType::None; // 十字道路类型
        pointBreakLU = POINT(0, 0);
        pointBreakLD = POINT(0, 0);
        pointBreakRU = POINT(0, 0);
        pointBreakRD = POINT(0, 0);

        uint16_t counterRec = 0;    // 计数器
        uint16_t counterLinear = 0; // 连续计数器
        _index = 0;

        // cout << "左斜率方差:" << track.stdevLeft << " 右斜率方差:" << track.stdevRight << endl;
        // if (track.pointsEdgeRight.size() < ROWSIMAGE / 2 || track.pointsEdgeLeft.size() < ROWSIMAGE / 2) // 十字有效行限制
        //     return false;

        if (track.pointsEdgeRight.size() < 10 || track.pointsEdgeLeft.size() < 10) // 十字有效行限制
            return false;

        _index = 1;
        //----------------------------------------------------------------------------------------------------
        //[01] 左入十字处理
        if (track.stdevRight > 50)
        {
            // 通过色块突变-搜索十字类型
            for (int i = 2; i < track.widthBlock.size() - 10; i++)
            {
                // 左入十字判断
                if (track.pointsEdgeLeft[i].y < 2 && track.widthBlock[i].y > track.widthBlock[i - 2].y)
                    counterRec++;
                if (counterRec)
                {
                    counterLinear++;
                    if (counterLinear > 8)
                    {
                        counterLinear = 0;
                        counterRec = 0;
                    }
                    if (counterRec > 5)
                    {
                        crossroadType = CrossroadType::CrossroadLeft; // 左入十字
                        _index = 2;
                        break;
                    }
                }
            }
        }
        // //[01] 右入十字处理
        // if (track.stdevLeft > 50)
        // {
        //     // 通过色块突变-搜索十字类型
        //     for (int i = 2; i < track.widthBlock.size() - 10; i++)
        //     {
        //         // 右入十字判断
        //         if (track.pointsEdgeRight[i].y > COLSIMAGE - 10 && abs(track.widthBlock[i].y - track.widthBlock[i - 2].y) < 5)
        //             counterRec++;
        //         // cout<<"右入十字判断"<<track.pointsEdgeRight[i].y<<" "<<track.widthBlock[i].y <<" "<<track.widthBlock[i - 2].y<<endl;
        //         if (counterRec)
        //         {
        //             counterLinear++;
        //             if (counterLinear > 8)
        //             {
        //                 counterLinear = 0;
        //                 counterRec = 0;
        //             }
        //             if (counterRec > 5)
        //             {
        //                 crossroadType = CrossroadType::CrossroadRight; // 右入十字
        //                 _index = 2;
        //                 break;
        //             }
        //         }
        //     }
        // }
        // counterRec = 0;
        // if (crossroadType == CrossroadType::CrossroadRight) // 右入十字
        // {
        //     cout << "右入十字" << endl;
        //     uint16_t rowBreakLeftDown = searchBreakLeftDown(track.pointsEdgeLeft); // 搜索十字赛道突变行（左下）
        //     cout << "右入十字的左下拐点:" << track.pointsEdgeLeft[rowBreakLeftDown].x << " " << track.pointsEdgeLeft[rowBreakLeftDown].y << endl;
        //     cout << track.spurroad.size() << ' ' << rowBreakLeftDown << ' ' << track.pointsEdgeLeft[rowBreakLeftDown].x << endl;
        //     if (rowBreakLeftDown >= 0 && track.pointsEdgeLeft[rowBreakLeftDown].x > 50)
        //     {
        //         pointBreakLD = track.pointsEdgeLeft[rowBreakLeftDown]; // 左下突变点
        //         if (track.spurroad.size() > 0)                         //[Step-1] 搜索到岔路
        //         {
        //             int indexSP = 0;
        //             for (int i = 0; i < track.spurroad.size(); i++) // 若存在多个岔路点：搜索最优点
        //             {
        //                 cout << "获得右入拐点:" << track.spurroad[i].y << " " << track.spurroad[i].x << endl;
        //                 // 确保拐点在岔路口的下面
        //                 if (pointBreakLD.x >= track.spurroad[indexSP].x)
        //                 {
        //                     indexSP = i;
        //                 }
        //             }
        //             // for (int i = 0; i < track.spurroad.size(); i++) // 若存在多个岔路点：搜索最优点
        //             // {
        //             //     if (pointBreakLD.y > track.spurroad[i].y && pointBreakLD.x > track.spurroad[i].x)
        //             //     {
        //             //         if (pointBreakLD.x - track.spurroad[i].x < pointBreakLD.x - track.spurroad[indexSP].x)
        //             //         {
        //             //             indexSP = i;
        //             //         }
        //             //     }
        //             // }
        //             cout << pointBreakLD.y << " " << track.spurroad[indexSP].x << " " << pointBreakLD.x << " " << track.spurroad[indexSP].y << endl;
        //             if (pointBreakLD.x >= track.spurroad[indexSP].x)
        //             {
        //                 uint16_t rowEnd = rowBreakLeftDown; // 赛道重搜索行
        //                 for (int i = rowBreakLeftDown; i < track.pointsEdgeLeft.size(); i++)
        //                 {
        //                     if (track.pointsEdgeLeft[i].y <= track.spurroad[indexSP].y)
        //                     {
        //                         rowEnd = i - 1;
        //                         break;
        //                     }
        //                 }
        //                 if(rowEnd == 0){
        //                     rowEnd = 1;
        //                 }
        //                 POINT startPoint = pointBreakLD;          // 补线起点
        //                 POINT endPoint = track.spurroad[indexSP]; // 补线终点
        //                 cout << "左边补线终点" << endPoint.x << " " << endPoint.y << endl;
        //                 POINT midPoint = POINT((startPoint.x + endPoint.x) * 0.5, (startPoint.y + endPoint.y) * 0.5); // 补线中点
        //                 vector<POINT> input = {startPoint, midPoint, endPoint};
        //                 vector<POINT> repair = Bezier(0.04, input);

        //                 track.pointsEdgeLeft.resize(rowBreakLeftDown); // 重绘左边缘
        //                 for (int i = 0; i < repair.size(); i++)
        //                 {
        //                     track.pointsEdgeLeft.push_back(repair[i]);
        //                 }

        //                 if (track.spurroad[indexSP].y > COLSIMAGE / 8)
        //                     track.trackRecognition(true, rowEnd); // 赛道边缘重新搜索
        //                 else
        //                     track.pointsEdgeRight.resize(rowEnd);

        //                 track.trackRecognition(true, rowEnd); // 赛道边缘重新搜索
        //                 repaired = true;                      // 补线成功
        //                 _index = 3;
        //             }
        //         }

        //         if (!repaired && pointBreakLD.y < COLSIMAGE / 2) //[Step-2] 未搜索到岔路
        //         {
        //             _index = 4;
        //             uint16_t rowBreakRU = rowBreakLeftDown; // 右上拐点
        //             for (int i = rowBreakLeftDown; i < track.pointsEdgeRight.size() - 10; i++)
        //             {
        //                 if (track.pointsEdgeRight[i].y > 1)
        //                     counterRec++;
        //                 else
        //                     counterRec = 0;
        //                 if (counterRec > 2)
        //                 {
        //                     rowBreakRU = i - 2;
        //                     break;
        //                 }
        //             }

        //             POINT endPoint = POINT(pointBreakLD.x, 1); // 补线终点
        //             if (rowBreakRU < track.pointsEdgeRight.size())
        //             {
        //                 pointBreakRU = track.pointsEdgeRight[rowBreakRU];           // 右上拐点
        //                 endPoint = POINT((pointBreakLD.x + pointBreakRU.x) / 2, 1); // 补线终点
        //                 track.pointsEdgeRight.resize(rowBreakRU);                   // 重绘右边缘
        //             }

        //             POINT startPoint = pointBreakLD;                                                              // 补线起点
        //             POINT midPoint = POINT((startPoint.x + endPoint.x) * 0.5, (startPoint.y + endPoint.y) * 0.5); // 补线中点
        //             vector<POINT> input = {startPoint, midPoint, endPoint};
        //             vector<POINT> repair = Bezier(0.05, input);

        //             track.pointsEdgeLeft.resize(rowBreakLeftDown); // 重绘左边缘
        //             for (int i = 0; i < repair.size(); i++)
        //             {
        //                 track.pointsEdgeLeft.push_back(repair[i]);
        //             }
        //             repaired = true; // 补线成功
        //         }
        //     }
        // }

        // 直入十字处理
        if (!repaired) // 如果写入十字未成功
        {
            cout << "直入十字处理" << endl;
            // 通过色块突变-搜索十字类型
            uint16_t counterStrightA = 0; // 计数器
            uint16_t counterStrightB = 0; // 计数器
            for (int i = 2; i < track.widthBlock.size() - 10; i++)
            {
                // 直入十字判断
                if (track.spurroad.size() > 0 && track.widthBlock[i].y > COLSIMAGE - 5)
                {
                    counterStrightA++;
                }
                if (counterStrightA)
                {
                    counterStrightB++;
                    if (counterStrightB > 30)
                    {
                        counterStrightB = 0;
                        counterStrightA = 0;
                    }
                    if (counterStrightA > 20) // 连续20行全宽
                    {
                        crossroadType = CrossroadType::CrossroadStraight; // 直入十字
                        _index = 5;
                        break;
                    }
                }
            }

            if (crossroadType == CrossroadType::CrossroadStraight) // 直入十字
            {
                int indexSP = 0;
                for (int i = 0; i < track.spurroad.size(); i++) // 挑选准确的岔路点
                {
                    if (track.spurroad[i].x > track.spurroad[indexSP].y && track.spurroad[i].x < pointBreakRD.x)
                    {
                        indexSP = i;
                        break;
                    }
                }

                // 搜索左边缘
                uint16_t rowBreakLU = searchBreakLeftUp(track.pointsEdgeLeft);   // 左上拐点搜索
                uint16_t rowBreakLD = searchBreakLeftDown(track.pointsEdgeLeft); // 左下拐点搜索

                // 优化左边缘
                if (rowBreakLU > rowBreakLD && rowBreakLU < COLSIMAGE / 2 && rowBreakLD < COLSIMAGE / 2)
                {
                    pointBreakLU = track.pointsEdgeLeft[rowBreakLU];
                    pointBreakLD = track.pointsEdgeLeft[rowBreakLD];

                    double k = (double)(pointBreakLU.y - pointBreakLD.y) /
                               (double)(pointBreakLU.x - pointBreakLD.x);
                    double b = pointBreakLU.y - k * pointBreakLU.x;
                    for (int i = rowBreakLD; i <= rowBreakLU; i++)
                    {
                        track.pointsEdgeLeft[i].y = (int)(k * track.pointsEdgeLeft[i].x + b);
                    }
                    repaired = true; // 补线成功
                }

                if (track.spurroad[indexSP].y > COLSIMAGE / 3)
                {
                    // 补右边线
                    //  重新进行Track搜索
                    uint16_t rowEnd = 10;
                    for (int i = 10; i < track.pointsEdgeRight.size(); i++)
                    {
                        if (track.pointsEdgeRight[i].x <= track.spurroad[indexSP].x)
                        {
                            rowEnd = i - 1;
                            break;
                        }
                    }

                    POINT startPoint = track.pointsEdgeRight[0]; // 补线起点

                    // 防止左右两边补线连接
                    // 最小的边缘线都大于岔路口，说明不是右边的岔路口
                    POINT endPoint;
                    // cout<<track.pointsEdgeRight[track.pointsEdgeRight.size()-1].y <<" w " << track.spurroad[indexSP].y<<endl;
                    if (track.pointsEdgeRight[track.pointsEdgeRight.size() / 2].y > track.spurroad[indexSP].y)
                    {
                        // cout<<"进来";
                        // 寻找割裂最大的边
                        int indexEND = 0;
                        for (int i = 1; i < track.pointsEdgeRight.size() - 1; i++)
                        {
                            if (track.pointsEdgeRight[i].y > track.pointsEdgeRight[i - 1].y  && track.pointsEdgeRight[i].x < startPoint.x)
                            {
                                indexEND = i;
                            }
                        }
                        endPoint = track.pointsEdgeRight[indexEND]; // 都找不到就直接补线终点
                    }
                    else
                    {
                        endPoint = track.spurroad[indexSP]; // 补线终点
                    }

                    // for (int i = 1; i < track.pointsEdgeRight.size()-1; i++)
                    //     {
                    //         if(abs(track.pointsEdgeRight[i].y - track.pointsEdgeRight[i+1].y) > 20){
                    //           endPoint = track.pointsEdgeRight[i];
                    //           break;
                    //         }else{
                    //           endPoint = track.pointsEdgeRight[track.pointsEdgeRight.size()-1]; // 都找不到就直接补线终点
                    //         }
                    //     }
                    // //当岔路点其中一个点线很近的时候，说明是该边的岔路
                    // for(int i =0;i<track.pointsEdgeRight.size()-1;i++){
                    //    if(abs(track.spurroad[indexSP].y - track.pointsEdgeRight[i].y) < 5 && abs(track.spurroad[indexSP].x - track.pointsEdgeRight[i].x) < 5){
                    //       endPoint = track.spurroad[indexSP]; // 补线终点
                    //       break;
                    //    }
                    // }

                    // cout<<"右边补的是"<<endPoint.x<<" H "<<endPoint.y<<endl;
                    POINT midPoint = POINT((startPoint.x + endPoint.x) * 0.5, (startPoint.y + endPoint.y) * 0.5); // 补线中点
                    vector<POINT> input = {startPoint, midPoint, endPoint};
                    vector<POINT> repair = Bezier(0.04, input);
                    // 记录右边上补角
                    POINT newEndPoint = endPoint;

                    track.pointsEdgeRight.clear(); // 重绘右边缘
                    track.pointsEdgeRight = repair;

                    track.trackRecognition(true, rowEnd); // 赛道边缘重新搜索

                    // 补左边线
                    indexSP = 0;
                    for (int i = 0; i < track.spurroad.size(); i++) // 挑选准确的岔路点
                    {
                        // 判断一定是在左边点
                        if (track.spurroad[i].y < track.spurroad[indexSP].y && track.spurroad[i].y < COLSIMAGE / 2)
                        {
                            indexSP = i;
                            // cout << track.spurroad[i].y << "  " << pointBreakLD.x << endl;
                        }
                    }

                    rowEnd = 10;
                    for (int i = 10; i < track.pointsEdgeLeft.size(); i++)
                    {
                        if (track.pointsEdgeLeft[i].x <= track.spurroad[indexSP].x)
                        {
                            rowEnd = i - 1;
                            break;
                        }
                    }

                    // 防止左右两边补线连接
                    // 最大的边缘线都小于岔路口，说明不是左边的岔路口
                    if (newEndPoint.x != track.spurroad[indexSP].x && newEndPoint.y != track.spurroad[indexSP].y)
                    {
                        endPoint = track.spurroad[indexSP]; // 补线终点
                    }
                    else if (track.pointsEdgeLeft[0].y < track.spurroad[indexSP].y)
                    {
                        // 寻找割裂最大的边
                        for (int i = 0; i < track.pointsEdgeLeft.size() - 1; i++)
                        {
                            if (abs(track.pointsEdgeLeft[i].y - track.pointsEdgeLeft[i + 1].y) > 20)
                            {
                                endPoint = track.pointsEdgeLeft[i + 1];
                                break;
                            }
                        }
                        endPoint = track.pointsEdgeLeft[track.pointsEdgeLeft.size() - 1]; // 都找不到就直接补线终点
                    }
                    else
                    {
                        endPoint = track.spurroad[indexSP]; // 补线终点
                    }

                    startPoint = track.pointsEdgeLeft[0]; // 补线起点
                                       POINT fixedEndPoint;
                    fixedEndPoint.x = 0;
                    fixedEndPoint.y = endPoint.y + static_cast<float>(endPoint.x) / ((startPoint.x - endPoint.x) / static_cast<float>(endPoint.y - startPoint.y));
                    rowEnd = 10;
                    for (int i = 10; i < track.pointsEdgeLeft.size(); i++)
                    {
                        if (track.pointsEdgeLeft[i].x <= track.spurroad[indexSP].x)
                        {
                            rowEnd = i - 1;
                            break;
                        }
                    }

                    // if (track.spurroad[indexSP].y > ROWSIMAGE / 2)
                    // {
                    //     endPoint = POINT(track.spurroad[indexSP].x, ROWSIMAGE - track.spurroad[indexSP].y);
                    // }
                    // else
                    // {
                    //     endPoint = track.spurroad[indexSP]; // 补线终点
                    // }
                    // cout << track.spurroad[indexSP].y << " 补线终点 " << track.spurroad[indexSP].x << endl;
                    // endPoint = POINT(0, startPoint.y +( (endPoint.x*endPoint.y)/(startPoint.x - endPoint.x)));
 
                    midPoint = POINT((startPoint.x + fixedEndPoint.x) * 0.5, (startPoint.y + fixedEndPoint.y) * 0.5); // 补线中点
                    cout << fixedEndPoint.x << " dd " << fixedEndPoint.y << endl;
                    input = {startPoint, midPoint, fixedEndPoint};
                    repair = Bezier(0.04, input);
                    track.pointsEdgeLeft.clear(); // 重绘左边缘
                    track.pointsEdgeLeft = repair;
                    track.trackRecognition(true, rowEnd); // 赛道边缘重新搜索

                    repaired = true; // 补线成功
                }
            }
        }

        return repaired;
    }

    /**
     * @brief 绘制十字道路识别结果
     *
     * @param Image 需要叠加显示的图像/RGB
     */
    void drawImage(Tracking track, Mat &Image)
    {
        // 绘制边缘点
        for (int i = 0; i < track.pointsEdgeLeft.size(); i++)
        {
            circle(Image, Point(track.pointsEdgeLeft[i].y, track.pointsEdgeLeft[i].x), 2,
                   Scalar(0, 255, 0), -1); // 绿色点
        }
        for (int i = 0; i < track.pointsEdgeRight.size(); i++)
        {
            circle(Image, Point(track.pointsEdgeRight[i].y, track.pointsEdgeRight[i].x), 2,
                   Scalar(0, 255, 255), -1); // 黄色点
        }

        // 绘制岔路点
        for (int i = 0; i < track.spurroad.size(); i++)
        {
            circle(Image, Point(track.spurroad[i].y, track.spurroad[i].x), 6,
                   Scalar(0, 0, 255), -1); // 红色点
            // cout << track.spurroad[i].y << " " << track.spurroad[i].x << endl;
        }

        // 斜入十字绘制补线起止点
        if (crossroadType == CrossroadType::CrossroadRight) // 右入十字
        {
            circle(Image, Point(pointBreakLU.y, pointBreakLU.x), 5, Scalar(226, 43, 138), -1); // 上补线点：紫色
            circle(Image, Point(pointBreakLD.y, pointBreakLD.x), 5, Scalar(255, 0, 255), -1);  // 下补线点：粉色
            if (pointBreakRU.x > 0)
                circle(Image, Point(pointBreakRU.y, pointBreakRU.x), 5, Scalar(226, 43, 138), -1); // 上补线点：紫色
            if (pointBreakRD.x > 0)
                circle(Image, Point(pointBreakRD.y, pointBreakRD.x), 5, Scalar(255, 0, 255), -1); // 下补线点：粉色

            putText(Image, "Right", Point(COLSIMAGE / 2 - 15, ROWSIMAGE - 20), cv::FONT_HERSHEY_TRIPLEX, 0.3, cv::Scalar(0, 255, 0), 1, CV_AA);
        }
        else if (crossroadType == CrossroadType::CrossroadLeft) // 左入十字
        {
            circle(Image, Point(pointBreakRU.y, pointBreakRU.x), 5, Scalar(226, 43, 138), -1); // 上补线点：紫色
            circle(Image, Point(pointBreakRD.y, pointBreakRD.x), 5, Scalar(255, 0, 255), -1);  // 下补线点：粉色
            if (pointBreakLU.x > 0)
                circle(Image, Point(pointBreakLU.y, pointBreakLU.x), 5, Scalar(226, 43, 138), -1); // 上补线点：紫色
            if (pointBreakLD.x > 0)
                circle(Image, Point(pointBreakLD.y, pointBreakLD.x), 5, Scalar(255, 0, 255), -1); // 下补线点：粉色

            putText(Image, "Left", Point(COLSIMAGE / 2 - 15, ROWSIMAGE - 20), cv::FONT_HERSHEY_TRIPLEX, 0.3, cv::Scalar(0, 255, 0), 1, CV_AA);
        }
        else if (crossroadType == CrossroadType::CrossroadStraight) // 直入十字
        {
            circle(Image, Point(pointBreakLU.y, pointBreakLU.x), 5, Scalar(226, 43, 138), -1); // 上补线点：紫色
            circle(Image, Point(pointBreakLD.y, pointBreakLD.x), 5, Scalar(255, 0, 255), -1);  // 下补线点：粉色
            circle(Image, Point(pointBreakRU.y, pointBreakRU.x), 5, Scalar(226, 43, 138), -1); // 上补线点：紫色
            circle(Image, Point(pointBreakRD.y, pointBreakRD.x), 5, Scalar(255, 0, 255), -1);  // 下补线点：粉色
            putText(Image, "Straight", Point(COLSIMAGE / 2 - 20, ROWSIMAGE - 20), cv::FONT_HERSHEY_TRIPLEX, 0.3, cv::Scalar(0, 255, 0), 1, CV_AA);
        }

        putText(Image, "[6] CROSS - ENABLE", Point(COLSIMAGE / 2 - 30, 10), cv::FONT_HERSHEY_TRIPLEX, 0.3, cv::Scalar(0, 255, 0), 1, CV_AA);
        putText(Image, to_string(_index), Point(COLSIMAGE / 2 - 5, ROWSIMAGE - 40), cv::FONT_HERSHEY_TRIPLEX, 0.5, cv::Scalar(0, 0, 155), 1, CV_AA);
    }

private:
    int _index = 0; // 测试

    POINT pointBreakLU;
    POINT pointBreakLD;
    POINT pointBreakRU;
    POINT pointBreakRD;
    uint16_t counterFild = 0;
    /**
     * @brief 十字道路类型
     *
     */
    enum CrossroadType
    {
        None = 0,
        CrossroadLeft,     // 左斜入十字
        CrossroadRight,    // 右斜入十字
        CrossroadStraight, // 直入十字
    };

    CrossroadType crossroadType = CrossroadType::None; // 十字道路类型

    /**
     * @brief 搜索十字赛道突变行（左上）
     *
     * @param pointsEdgeLeft
     * @return uint16_t
     */
    uint16_t searchBreakLeftUp(vector<POINT> pointsEdgeLeft)
    {
        uint16_t rowBreakLeftUp = pointsEdgeLeft.size() - 5;
        uint16_t counter = 0;
        uint16_t counterFilter = 0;
        for (int i = pointsEdgeLeft.size() - 5; i > 50; i--)
        {
            if (pointsEdgeLeft[i].y > 2 & abs(pointsEdgeLeft[i].y - pointsEdgeLeft[i + 1].y) < 3)
            {
                rowBreakLeftUp = i;
                counter = 0;
                counterFilter++;
            }
            else if (pointsEdgeLeft[i].y <= 2 && counterFilter > 10)
            {
                counter++;
                if (counter > 5)
                    return rowBreakLeftUp;
            }
        }

        return rowBreakLeftUp;
    }
    /**
     * @brief 搜索十字赛道突变行（左下）
     *
     * @param pointsEdgeLeft
     * @return uint16_t
     */
    uint16_t searchBreakLeftDown(vector<POINT> pointsEdgeLeft)
    {
        uint16_t rowBreakLeft = 0;
        uint16_t counter = 0;
        for (int i = 0; i < pointsEdgeLeft.size(); i++) // 寻找左边跳变点
        {
            // cout << "左边缘点集合:" << pointsEdgeLeft[i].x << " " << pointsEdgeLeft[i].y << endl;
            // 找到角点的那个点,点集是x从大到小排列的,取最大的x,同时左右的y要在附近,一满足就马上退出
            if (pointsEdgeLeft.size() > 1)
            {
                if (abs(pointsEdgeLeft[i].y - pointsEdgeLeft[i + 1].y) < 10)
                {
                    rowBreakLeft = i;
                    break;
                }
            }
            // else if (pointsEdgeLeft[i].y <= pointsEdgeLeft[rowBreakLeft].y) // 突变点计数
            // {
            //     counter++;
            //     if (counter > 5)
            //         return rowBreakLeft;
            // }
        }

        return rowBreakLeft;
    }
    /**
     * @brief 搜索十字赛道突变行（右上）
     *
     * @param pointsEdgeRight
     * @return uint16_t
     */
    uint16_t searchBreakRightUp(vector<POINT> pointsEdgeRight)
    {
        uint16_t rowBreakRightUp = pointsEdgeRight.size() - 5;
        for (int i = pointsEdgeRight.size() - 5; i > 5; i--)
        {
            if (pointsEdgeRight[i].y < COLSIMAGE - 2 && abs(pointsEdgeRight[i].y - pointsEdgeRight[i + 1].y) < 3 && abs(pointsEdgeRight[i].x - pointsEdgeRight[i + 1].x < 3) && abs(pointsEdgeRight[i].x - pointsEdgeRight[i - 1].x < 3))
            {
                rowBreakRightUp = i;
            }
        }

        return rowBreakRightUp;
    }
    /**
     * @brief 搜索十字赛道突变行（右下）
     *
     * @param pointsEdgeRight
     * @return uint16_t
     */
    // uint16_t searchBreakRightDown(vector<POINT> pointsEdgeRight)
    // {
    //     uint16_t rowBreakRightDown = 0;
    //     uint16_t counter = 0;
    //     bool start = false;

    //     for (int i = 0; i < pointsEdgeRight.size() - 10; i++) // 寻找左边跳变点
    //     {
    //         if (pointsEdgeRight[i].y < COLSIMAGE - 1)
    //             counter++;
    //         else
    //             counter = 0;

    //         if (counter > 2)
    //         {
    //             start = true;
    //             counter = 0;
    //         }

    //         if (start) // 屏蔽初始行
    //         {
    //             // 判定点的y最小，以及密度的值不能过于小
    //             if (pointsEdgeRight[i].y < pointsEdgeRight[i - 2].y)
    //                 counter++;
    //             else
    //                 counter = 0;

    //             if (counter > 2)
    //             {
    //                 return i - 3;
    //             }
    //         }
    //     }

    //     return rowBreakRightDown;
    // }

    /**
     * @brief 搜索十字赛道突变行（右下）右下角点检测
     *
     * @param pointsEdgeRight
     *
     * @return    返回角点所在的行数，找不到返回0
     *Sample     left_down_guai[0]=Find_Left_Down_Point(MT9V03X_H-1,20);
     * @note      角点检测阈值可根据实际值更改
     */
    int searchBreakRightDown(vector<POINT> pointsEdgeRight) // 找右下角点，返回值是角点所在的行数
    {
        int i, t;
        int right_down_line = 0;
        int start = 5;
        int end = pointsEdgeRight.size() - 5;
        if (end <= 5)
            end = start + 1;
        for (i = end; i >= start; i--)
        {
            if (right_down_line == 0 &&                                      // 只找第一个符合条件的点
                abs(pointsEdgeRight[i].y - pointsEdgeRight[i + 1].y) <= 5 && // 角点的阈值可以更改
                abs(pointsEdgeRight[i + 1].y - pointsEdgeRight[i + 2].y) <= 5 &&
                abs(pointsEdgeRight[i + 2].y - pointsEdgeRight[i + 3].y) <= 5 &&
                (pointsEdgeRight[i].y - pointsEdgeRight[i - 2].y) >= 5 &&
                (pointsEdgeRight[i].y - pointsEdgeRight[i - 3].y) >= 10 &&
                (pointsEdgeRight[i].y - pointsEdgeRight[i - 4].y) >= 10 && abs(pointsEdgeRight[i].x - pointsEdgeRight[i - 1].x) <= 3 && abs(pointsEdgeRight[i].x - pointsEdgeRight[i + 1].x) <= 3)
            {
                right_down_line = i; // 获取行数即可
                break;
            }
        }
        return right_down_line;
    }

    /**
     * @brief 直入十字搜索
     *
     * @param pointsEdgeLeft 赛道左边缘点集
     * @param pointsEdgeRight 赛道右边缘点集
     * @return true
     * @return false
     */
    bool searchStraightCrossroad(vector<POINT> pointsEdgeLeft, vector<POINT> pointsEdgeRight)
    {
        if (pointsEdgeLeft.size() < ROWSIMAGE * 0.8 || pointsEdgeRight.size() < ROWSIMAGE * 0.8)
        {
            return false;
        }

        uint16_t counterLeft = 0;
        uint16_t counterRight = 0;
        for (int i = pointsEdgeLeft.size() - 10; i > 1; i--) // 搜索上半部分边缘点
        {
            if (pointsEdgeLeft[i].x > ROWSIMAGE / 2)
                break;
            else if (pointsEdgeLeft[i].y < 2)
                counterLeft++;
        }
        for (int i = pointsEdgeRight.size() - 10; i > 1; i--) // 搜索上半部分边缘点
        {
            if (pointsEdgeRight[i].x > ROWSIMAGE / 2)
                break;
            else if (pointsEdgeRight[i].y > COLSIMAGE - 2)
                counterRight++;
        }
        if (counterLeft > 30 && counterRight > 30)
            return true;
        else
            return false;
    }
};